<!DOCTYPE html>
<html>
<head>
    <title>SMACCMPilot Altitude Hold</title>

    <!-- Bootstrap core CSS -->
    <link href="/bootstrap-3.3.4-dist/css/bootstrap.min.css" rel="stylesheet">
    <!-- Bootstrap theme -->
    <link href="/bootstrap-3.3.4-dist/css/bootstrap-theme.min.css" rel="stylesheet">
</head>
<body role="document">
    <div class="container">

      <div class="row">

        <div class="col-md-3" role="main">
          <h2>Altitude Hold Debug</h2>
          <p><button type="button" class="btn btn-sm" id="alt-control-debug-btn">?</button>
          </p>

          <p>alt_est [m]:
          <span id="alt_est-lbl">?</span>
          </p>

          <p>alt_rate_est [m/s]:
          <span id="alt_rate_est-lbl">?</span>
          </p>

          <p>vz_ctl [0-1]:
          <span id="vz_ctl-lbl">?</span>
          </p>

          <p>ui_setp [m]:
          <span id="ui_setp-lbl">?</span>
          </p>

          <p>ui_rate_setp [m/s]:
          <span id="ui_rate_setp-lbl">?</span>
          </p>

        </div> <!-- end column -->

      </div> <!-- end row -->
      

      <div class="row">
        <div id="chartContainer1" style="height: 300px; width:100%;"></div>
        <div id="chartContainer2" style="height: 300px; width:100%;"></div>
        <div id="chartContainer3" style="height: 300px; width:100%;"></div>
        <div id="chartContainer4" style="height: 300px; width:100%;"></div>
      </div> <!-- end row -->

    </div> <!-- end container -->
    <script src="/underscore-1.8.3.js"></script>
    <script src="/jquery-2.1.3.js"></script>
    <script src="/backbone-1.1.2.js"></script>
    <script src="/bootstrap-3.3.4-dist/js/bootstrap.min.js"></script>
    <script src="/scheduler.js"></script>
    <script type="text/javascript" src="/canvasjs-1.9.1/canvasjs.min.js"></script>

<script type="text/javascript">
var labels = ["alt_est", "alt_rate_est", "r22_gain", "ui_setp","ui_rate_setp", "vz_fb","vz_fb_norm", "vz_ff", "vz_ff_rot", "vz_ctl"];
var data = []; // array of variables

	window.onload = function () {
		var dps0 = []; // alt est
 		var dps1 = []; // ui_setp

		var dps2 = []; // rate est
 		var dps3 = []; // rate setp

		var dps4 = []; // pos error
 		var dps5 = []; // pos rate error

 		var dps6 = []; // vz_ctl
 		var dps7 = []; // vz_fb
    var dps8 = []; // vz_fb_norm
    var dps9 = []; // vz_ff
    var dps10 = []; // vz_ff_rot

		var xVal = 0;
		var yVal = 0;	
		var updateInterval = 100; // in ms
		var dataLength = 500; // number of dataPoints visible at any point

    /*
    * CHART 1: alt_est + alt_ref
    */
		var chart1 = new CanvasJS.Chart("chartContainer1",{
			title :{
				text: "Altitude"
			},
			axisY: {
				title: "Altitude [m]",
        includeZero: false
			},		
			data: [{
        legendText: "alt_est",
				showInLegend: "true",
				type: "line",
				dataPoints: dps0 
  			}, 
        {
				legendText: "ui_setp",
  			showInLegend: "true",
				type: "line",
				dataPoints: dps1 
			}]
		});


    /*
    * CHART 2: alt_rate_est + alt_rate_ref
    */
		var chart2 = new CanvasJS.Chart("chartContainer2",{
			title :{
				text: "Altitude rate"
			},
			axisY: {
				title: "Altitude rate [m/s]"
			},		
			data: [{
        legendText: "alt_rate_est",
				showInLegend: "true",
				type: "line",
				dataPoints: dps2 
  			}, 
        {
				legendText: "ui_rate_setp",
  			showInLegend: "true",
				type: "line",
				dataPoints: dps3 
			}]
		});


    /*
    * CHART 3: alt_err + rate_err
    */
		var chart3 = new CanvasJS.Chart("chartContainer3",{
			title :{
				text: "Errors"
			},
			axisY: {
				title: "Errors"
			},		
			data: [{
        legendText: "pos_err",
				showInLegend: "true",
				type: "line",
				dataPoints: dps4 
  			}, 
        {
				legendText: "pos_rate_err",
  			showInLegend: "true",
				type: "line",
				dataPoints: dps5 
			}]
		});

    /*
    * CHART 4: vz_ctl
    */
		var chart4 = new CanvasJS.Chart("chartContainer4",{
			title :{
				text: "Control Action"
			},
			axisY: {
				title: "Input"
			},		
			data: [{
				legendText: "vz_ctl",
  			showInLegend: "true",
				type: "line",
				dataPoints: dps6 
			},
      {
				legendText: "vz_fb",
  			showInLegend: "true",
				type: "line",
				dataPoints: dps7 
			},
      {
				legendText: "vz_fb_norm",
  			showInLegend: "true",
				type: "line",
				dataPoints: dps8 
			},
      {
				legendText: "vz_ff",
  			showInLegend: "true",
				type: "line",
				dataPoints: dps9 
			},
      {
				legendText: "vz_ff_rot",
  			showInLegend: "true",
				type: "line",
				dataPoints: dps10 
			}]
		});


		var updateChart = function (count) {
			count = count || 1;
			// count is number of times loop runs to generate random dataPoints.
			
      
			for (var j = 0; j < count; j++) {
        // update xaxis
        xVal += updateInterval/1000.0;
        var idx = -1;

        /*
        * CHART 1: alt_est + alt_ref
        */
        // search for the right variable
        idx = match_labels("alt_est");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = 0; // backup 
        }

				dps0.push({
					x: xVal,
					y: yVal
				});

        // search for the right variable
        idx = match_labels("ui_setp");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = 0; // backup 
        }
        dps1.push({
					x: xVal,
					y: yVal
				});

        /*
        * CHART 2: alt_rate_est + alt_rate_ref
        */
        idx = match_labels("alt_rate_est");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = 0; // backup 
        }

				dps2.push({
					x: xVal,
					y: yVal
				});

        // search for the right variable
        idx = match_labels("ui_rate_setp");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = 0; // backup 
        }
        dps3.push({
					x: xVal,
					y: yVal
				});

        /*
        * CHART 3: alt_err & alt_rate_err
        */
        idx = match_labels("pos_err");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = 0; // backup 
        }

				dps4.push({
					x: xVal,
					y: yVal
				});

        // search for the right variable
        var idx = match_labels("pos_rate_err");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = -1; // backup 
        }
        dps5.push({
					x: xVal,
					y: yVal
				});

        // search for the right variable
        var idx = match_labels("vz_ctl");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = -1; // backup 
        }
        dps6.push({
					x: xVal,
					y: yVal
				});

        // search for the right variable
        var idx = match_labels("vz_fb");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = -1; // backup 
        }
        dps7.push({
					x: xVal,
					y: yVal
				});

        // search for the right variable
        var idx = match_labels("vz_fb_norm");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = -1; // backup 
        }
        dps8.push({
          x: xVal,
          y: yVal
        });

        // search for the right variable
        var idx = match_labels("vz_ff");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = -1; // backup 
        }
        dps9.push({
          x: xVal,
          y: yVal
        });

        // search for the right variable
        var idx = match_labels("vz_ff_rot");
        if (idx != -1) { // we have a match here
          yVal = data[idx];
        }
        else {
          yVal = -1; // backup 
        }
        dps10.push({
          x: xVal,
          y: yVal
        });


			}; // end for loop


      // assuming all dataLengths are the same...
			if (dps0.length > dataLength)
			{
				dps0.shift();
				dps1.shift();
				dps2.shift();
				dps3.shift();
				dps4.shift();
				dps5.shift();
				dps6.shift();
        dps7.shift();
        dps8.shift();
        dps9.shift();
        dps10.shift();
      }
			chart1.render();
			chart2.render();
			chart3.render();
			chart4.render();
		}; // updateChart

		// generates first set of dataPoints
		updateChart(dataLength); 

		// update chart after specified time. 
		setInterval(function(){updateChart()}, updateInterval); 
  }

function match_labels(my_label) {
  var idx = -1;
  for (i = 0; i < labels.length; i++) {
    var res = my_label.match(labels[i]);
    if (res != null) {
      idx = i;
      return idx;
    }
  }
  return idx;
}

$(function() {

var AltOutput = Backbone.Model.extend({
  urlRoot: '/controllable_vehicle_i/alt_control_debug'
});

var AltSliderView = Backbone.View.extend({
  initialize: function (options) {
    this.selector = options.selector;
    this.model.on('change', this.render, this);
    this.$label   = $('#' + this.selector + '-lbl');
    this.render();
  },
  render: function () {
    var val = this.model.toJSON()[this.selector] || 0;
    this.$label.html(val);
    var idx = match_labels(this.selector);
    if (idx != -1) { // we have a match here
      data[idx] = val; // save into global array
    }
  }
});


/*
(def-struct alt_control_debug_t -- 40+24 = 64 bytes
  (alt_est       float_t)
  (alt_rate_est  float_t)
  (r22_gain      float_t)
  (ui_setp       float_t)
  (ui_rate_setp  float_t)
  (pos           pid_state_t)
  (vz_fb         float_t)
  (vz_fb_norm    float_t)
  (vz_ff         float_t)
  (vz_ff_rot     float_t)
  (vz_ctl        float_t))

*/
var AltView = function (opts) {
  this.altEst =
    new AltSliderView({model: opts.model, selector: 'alt_est'}); // estimated altitude
  this.altRateEst =
    new AltSliderView({model: opts.model, selector: 'alt_rate_est'}); // estimated rate
  this.r22Gain =
    new AltSliderView({model: opts.model, selector: 'r22_gain'}); // derotation gain
  this.uiSetp =
    new AltSliderView({model: opts.model, selector: 'ui_setp'}); // altitude setpoint - aka ui_alt
  this.iuRateSetp =
    new AltSliderView({model: opts.model, selector: 'ui_rate_setp'}); // rate setpoint - typically zero
  this.vzFb =
    new AltSliderView({model: opts.model, selector: 'vz_fb'});
  this.vzFbNorm =
    new AltSliderView({model: opts.model, selector: 'vz_fb_norm'});
  this.vzFf =
    new AltSliderView({model: opts.model, selector: 'vz_ff'});
  this.vzFfRot =
    new AltSliderView({model: opts.model, selector: 'vz_ff_rot'});
  this.vzCmd =
    new AltSliderView({model: opts.model, selector: 'vz_ctl'}); // control input before the mixer (still can be overwritten by the position of throttle stick
};

window.altOutput = new AltOutput({});
window.altOutputView = new AltView({ model: altOutput });

window.altOutputScheduler =
  new Scheduler({ period: 100}, altOutput);
window.altOutputSchedulerView =
  new SchedulerButtonView({ model: altOutputScheduler, el: '#alt-control-debug-btn' });

});
</script>

</body>
</html>
